//
//FOC.C
// Created by Xiaoyun on 2023/3/28.
//
#include <stdint.h>
#include <stdio.h>
#include "foc.h"

PI_Param_t PI_q=PI_PARAM_DEFAUT_Q;
PI_Param_t PI_d=PI_PARAM_DEFAUT_D;

/**
 * @brief Init FOC BASE Functions
 *
 */
void FOC_Init() {
  FOC_Bsp_Init();
}

/**
 * @note need user to implement the function
 */
__weak void FOC_Bsp_Init() {

}

/**
 *
 * @param theta [float -180.,180. degrees]
 * @param sin [float* -1.,1. ]
 * @param cos [float* -1.,1. ]
 * @note need user to implement the function
 */
__weak void FOC_SinCos(float theta, float *sin, float *cos) {

}

/**
 *
 * @param pwm_a [float 0., 1. ]
 * @param pwm_b [float 0., 1. ]
 * @param pwm_c [float 0., 1. ]
 * @note need user to implement the function
 */
__weak void FOC_Set_PWM(float pwm_a, float pwm_b, float pwm_c) {

}

/**
 *
 * @return [float -180.,180. degrees]
 * @note need user to implement the function
 */
__weak float FOC_Get_Angle() {

}
/**
 *
 * @param ia [float* A] : output
 * @param ib [float* A] : output
 * @param ic [float* A] : output
 * @note need user to implement the function
 */
__weak void FOC_Get_Current(float* ia,float* ib,float* ic){

}
/**
 * @brief Set the trig when to start ADC
 * @return [float 0, 1]
 * @note need user to implement the function
 */
__weak void FOC_Set_ADC_Trig(float trig_phase){

}
/**
 * @brief convert Mechanical angle to Electric angle
 * @return [float -180.,180. degrees]
 */
float FOC_Get_Elec_Angle() {
  float e_angle = FOC_Get_Angle() * (FOC_MOTOR_N_POLES / 2);
  while (e_angle <= -180 || 180 < e_angle) {
    if (e_angle <= -180)e_angle += 360;
    else e_angle -= 360;
  }
  return e_angle;
}

/**
 *
 * @brief Open-loop control function, contains Inverse Park Trans and SVPWM
 * @param uq [float 0., 1. ] : input
 * @param ud [float 0., 1. ] : input
 * @param sin_th [float -1., 1.] : input
 * @param cos_th [float -1., 1.] : input
 * @param pwma [float* -1., 1.] : output
 * @param pwmb [float* -1., 1.] : output
 * @param pwmc [float* -1., 1.] : output
 * @return The sector where the magnetic vector is located
 */
uint8_t FOC_InvPark_SVPWM(float uq, float ud,float sin_th,float cos_th,float* pwma,float* pwmb,float* pwmc) {
  static float angle, ualpa, ubeta, uref1, uref2, uref3;
//  angle=FOC_Get_Elec_Angle();
//  FOC_SinCos(angle, &sin_th, &cos_th);
  ualpa = ud * cos_th - uq * sin_th;
  ubeta = ud * sin_th + uq * cos_th;
  uref1 = ubeta;
  uref2 = -0.5f * C_SQRT_3 * ualpa - 0.5f * ubeta;
  uref3 = 0.5f * C_SQRT_3 * ualpa - 0.5f * ubeta;

  uint8_t selector = (uref1 > 0 ? 0x1 : 0x0) | (uref3 > 0 ? 0x2 : 0x0) | (uref2 > 0 ? 0x4 : 0x0);
  uint8_t sector=0;
  *pwma=*pwmb=*pwmc=0;
  switch (selector) {
    case 3: {
      sector=1;
      *pwmb = uref1*2/C_SQRT_3;
      *pwma = uref3*2/C_SQRT_3+*pwmb;
      *pwmc = (1-*pwma)/2;
      *pwma += *pwmc;
      *pwmb += *pwmc;
    }
      break;
    case 1: {
      sector=2;
      *pwma = -uref2*2/C_SQRT_3;
      *pwmb = -uref3*2/C_SQRT_3+*pwma;
      *pwmc = (1-*pwmb)/2;
      *pwmb += *pwmc;
      *pwma += *pwmc;
    }
      break;
    case 5: {
      sector=3;
      *pwmc = uref2*2/C_SQRT_3;
      *pwmb = uref1*2/C_SQRT_3+*pwmc;
      *pwma = (1-*pwmb)/2;
      *pwmb += *pwma;
      *pwmc += *pwma;
    }
      break;
    case 4: {
      sector=4;
      *pwmb = -uref3*2/C_SQRT_3;
      *pwmc = -uref1*2/C_SQRT_3+*pwmb;
      *pwma = (1-*pwmc)/2;
      *pwmc += *pwma;
      *pwmb += *pwma;
    }
      break;
    case 6: {
      sector=5;
      *pwma = uref3*2/C_SQRT_3;
      *pwmc = uref2*2/C_SQRT_3+*pwma;
      *pwmb = (1-*pwmc)/2;
      *pwmc += *pwmb;
      *pwma += *pwmb;
    }
      break;
    case 2: {
      sector=6;
      *pwmc = -uref1*2/C_SQRT_3;
      *pwma = -uref2*2/C_SQRT_3+*pwmc;
      *pwmb = (1-*pwma)/2;
      *pwma += *pwmb;
      *pwmc += *pwmb;
    }
      break;
    default:
      break;
  }
  return sector;
}

/**
 *
 * @brief Current feedback function
 * @param ia [float A] : input
 * @param ib [float A] : input
 * @param ic [float A] : input
 * @param sin_th [float -1., 1.] : input
 * @param cos_th [float -1., 1.] : input
 * @param iq [float* 0., 1.] : output
 * @param id [float* 0., 1.] : output
 */
void FOC_Clark_Park(float ia,float ib,float ic,float sin_th,float cos_th,float* iq,float* id){
  static float angle, ialpha, ibeta,iq_prev,id_prev;
//  angle=FOC_Get_Elec_Angle();
//  FOC_SinCos(angle, &sin_th, &cos_th);

  // Clark Transform
  ialpha=ia-0.5f*ib-0.5f*ic;
  ibeta = C_SQRT_3*0.5f*(ib-ic);

  // Park Transform
  *iq=-ialpha*sin_th+ibeta*cos_th;
  *id=ialpha*cos_th+ibeta*sin_th;

  // Low Pass Filter
  *iq=LOW_PASS_FILTER_C*iq_prev+(1.f-LOW_PASS_FILTER_C)*(*iq);
  *id=LOW_PASS_FILTER_C*id_prev+(1.f-LOW_PASS_FILTER_C)*(*id);

  iq_prev=*iq;
  id_prev=*id;
}

// Limit
static float Limit(float *x,float min,float max){
  if(*x>max)*x=max;
  if(*x<min)*x=min;
  return *x;
}

/**
 * @brief PI controller of FOC
 * @param iqs [float A] : input setting value
 * @param ids [float A] : input setting value
 * @param iq [float A] : input feedback value
 * @param id [float A] : input feedback value
 * @param uq [float* 0., 1.] : output
 * @param ud [float* 0., 1.] : output
 */
void FOC_PI(float iqs,float ids,float iq,float id,float *uq,float *ud){
  static float I_outq, I_outd;
  float outq=0;
  float outd=0;
  float eq=iqs-iq;
  float ed=ids-id;
  outq = PI_q.P*eq;
  outd = PI_d.P*ed;
  I_outq +=PI_q.P*PI_q.I*eq;
  Limit(&I_outq,-0.5f,0.5f);
  I_outd +=PI_d.P*PI_d.I*ed;
  Limit(&I_outd,-0.5f,0.5f);
  outq += I_outq;
  Limit(&outq,-1,1);
  outd += I_outd;
  Limit(&outd,-1,1);
  *uq = outq;
  *ud = outd;
}

/**
 * @brief Used when Calibration Motor Angle
 */
void FOC_Angle_Calibration() {
  FOC_Set_PWM(0.2f, 0, 0);
}

/**
 * @brief Closed-loop control function, contains Current control loop
 */
void FOC_Closed_Loop(float iq_set,float id_set){
  volatile static float angle,sin_th,cos_th,ia,ib,ic,iq,id,uq,ud,pwma,pwmb,pwmc;
  volatile static uint8_t sector;
  angle=FOC_Get_Elec_Angle();
  FOC_SinCos(angle, (float*)&sin_th, (float*)&cos_th);
  FOC_Get_Current((float*)&ia,(float*)&ib,(float*)&ic);
  switch (sector) {
    case 1: case 6: ia=-ib-ic;break;
    case 2: case 3: ib=-ia-ic;break;
    case 4: case 5: ic=-ia-ib;break;
    default:
      break;
  }
  FOC_Clark_Park(ia,ib,ic,sin_th,cos_th,(float*)&iq,(float*)&id);
  FOC_PI(iq_set,id_set,iq,id,(float*)&uq,(float*)&ud);
  sector=FOC_InvPark_SVPWM(uq, ud,sin_th,cos_th,(float*)&pwma,(float*)&pwmb,(float*)&pwmc);
//  switch (sector) {
//    case 1:case 4:FOC_Set_ADC_Trig(pwmb/2.f);break;
//    case 2:case 5:FOC_Set_ADC_Trig(pwma/2.f);break;
//    case 3:case 6:FOC_Set_ADC_Trig(pwmc/2.f);break;
//    default:FOC_Set_ADC_Trig(0.5f);break;
//  }

  FOC_Set_PWM(pwma,pwmb,pwmc);
}